/*
The data are made up of data frames. Each data frame contains 20 bytes (8 bits for each byte).

23 23 00 4D 00 02 00 22 03 BA FF E4 FE AA FA 9B 00 7F 02 1B
----- ----- ----- ----- ----- ----- ----- ----- ----- -----
0     1     2     3     4     5     6     7     8     9

The first two '23 23' are the header of each frame.
The next 18 bytes are the measurements from three sensors(three axes for each sensor
and totally three sensors). Each measurements contains two bytes and the most significant
appears first. And the order of the measurements are X-axis of gyroscope, Y-axis of gyroscope,
Z-axis of gyroscope, X-axis of accelerometer, Y-axis of accelerometer, Z-axis of accelerometer,
X-axis of magnetometer, Y-axis of magnetometer, Z-axis of magnetometer. For example, in the data
frame I have shown

the measurement of X-axis of gyroscope is 0x004D (HEX).
the measurement of Y-axis of accelerometer is 0xFFE4 (HEX), which is represented using two's complement.

As to how to convert these measurement to the real acceleration and angular rate,
You need to check the data sheet of the three sensors( ITG3200 LIS3LV02DL HMC5843 ).
 */
package acelab.bluetooth;

import acelab.tonegeneration.SoundOutput;
import gnu.io.CommPort;
import gnu.io.CommPortIdentifier;
import gnu.io.SerialPort;
import java.io.File;
import java.io.FileInputStream;
import java.io.FileOutputStream;
import java.io.FileWriter;

import java.io.IOException;
import java.io.InputStream;
import java.io.OutputStream;
import java.text.SimpleDateFormat;
import java.util.ArrayDeque;
import java.util.Calendar;
import java.util.Date;
import java.util.LinkedList;
import java.util.TimeZone;
import java.util.logging.Level;
import java.util.logging.Logger;
import java.util.prefs.Preferences;
import javax.swing.JOptionPane;
import sun.io.CharToByteConverter;
import tonegen.ToneGenView;

/**
 *
 * @author Administrator
 */
public final class DeviceCommunication {

    static SoundOutput sound;
    InputStream in;
    OutputStream out;
    static ToneGenView view;
    static Boolean running = false;
    KalmanFilter kf;
    static FileWriter output = null;
    String filePath = "";
    public static LinkedList<Double> thresholds = new LinkedList<Double>();
    public static LinkedList<Double> frequencies = new LinkedList<Double>();
    static SimpleDateFormat fmt = new SimpleDateFormat("yyyy-MM-dd HH:mm:ss.SS");
    int trial = 0;

    public void setThresholds(LinkedList<Double> t, LinkedList<Double> f) {
        thresholds.clear();
        frequencies.clear();
        thresholds = t;
        frequencies = f;
    }

    public DeviceCommunication(int port) {

        fmt.setTimeZone(TimeZone.getTimeZone("EST"));
        kf = new KalmanFilter();
        float[] initialValues = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
        kf.init(initialValues, 5);

        loadConfig();

        if (initialize(port) > 0) {
            sound = new SoundOutput();
        }
    }

    public void loadConfig() {
        Preferences prefsRoot = null;
        File config = new File("toneGenConfig.xml");
        if (config.canRead()) {
            // Retrieve preferences
            System.out.println("Reading config file toneGenConfig.xml");
            try {
                Preferences.importPreferences(new FileInputStream("toneGenConfig.xml"));
                prefsRoot = Preferences.userRoot().node("toneGen");

            } catch (Exception ex) {
                Logger.getLogger(DeviceCommunication.class.getName()).log(Level.SEVERE, null, ex);
            }
        } else {

            prefsRoot = Preferences.userRoot().node("toneGen");
            prefsRoot.putFloat("1 mG", 200.0f);
            prefsRoot.putFloat("1 Hz", 1000.0f);
            prefsRoot.putFloat("2 mG", 500.0f);
            prefsRoot.putFloat("2 Hz", 700.0f);
            prefsRoot.putFloat("3 mG", 1000.0f);
            prefsRoot.putFloat("3 Hz", 500.0f);
            prefsRoot.putFloat("4 mG", 2000.0f);
            prefsRoot.putFloat("4 Hz", 260.0f);
            prefsRoot.putFloat("5 mG", 5000.0f);
            prefsRoot.putFloat("5 Hz", 50.0f);

            FileOutputStream fos;
            try {
                fos = new FileOutputStream("toneGenConfig.xml");
                prefsRoot.exportSubtree(fos);
                fos.close();
                System.out.println("Wrote default config file toneGenConfig.xml");
            } catch (Exception ex) {
                Logger.getLogger(DeviceCommunication.class.getName()).log(Level.SEVERE, null, ex);
            }
        }

        thresholds.clear();
        frequencies.clear();
        if (prefsRoot != null) {
            try {
                for (String entry : prefsRoot.keys()) { // We should probably clear these values if we every support live config file loading
                    if (entry.endsWith("mG")) {
                        thresholds.add((double) prefsRoot.getFloat(entry, 0.0f));
                    } else { // ends with Hz
                        frequencies.add((double) prefsRoot.getFloat(entry, 0.0f));
                    }
                }
            } catch (Exception ex) {
                Logger.getLogger(DeviceCommunication.class.getName()).log(Level.SEVERE, null, ex);
            }
        } else {
            //init cutoffs and frequencies even without a config file
            thresholds.add(200.0);
            thresholds.add(500.0);
            thresholds.add(1000.0);
            thresholds.add(2000.0);
            thresholds.add(5000.0);
            frequencies.add(1000.0);
            frequencies.add(700.0);
            frequencies.add(500.0);
            frequencies.add(260.0);
            frequencies.add(50.0);
        }
    }

    public void saveConfig(LinkedList<Double> t, LinkedList<Double> f) {
        Preferences prefsRoot = null;
        File config = new File("toneGenConfig.xml");
        if (config.canRead()) {

            prefsRoot = Preferences.userRoot().node("toneGen");

            for (int x = 1; x < 6; x++) {
                prefsRoot.putFloat(x + " mG", t.get(x - 1).floatValue());
                prefsRoot.putFloat(x + " Hz", f.get(x - 1).floatValue());
            }

            FileOutputStream fos;
            try {
                fos = new FileOutputStream("toneGenConfig.xml");
                prefsRoot.exportSubtree(fos);
                fos.close();
                System.out.println("Wrote new config file toneGenConfig.xml");
            } catch (Exception ex) {
                Logger.getLogger(DeviceCommunication.class.getName()).log(Level.SEVERE, null, ex);
            }
        }
    }

    /**
     * http://rxtx.qbang.org/wiki/index.php/Two_way_communcation_with_the_serial_port
     */
    int initialize(int comPort) {
        try {
            if (comPort <= 0) {
                comPort = 3;
            }

            connect("COM" + comPort);
            return comPort;

        } catch (Exception e) {

            JOptionPane.showMessageDialog(null,
                    "Please verify that your device is connected to COM Port 3 and restart the program.",
                    "Problems opening COM Port " + comPort + ".",
                    JOptionPane.WARNING_MESSAGE);
            System.out.println(e.toString());
            return -1;
        }
    }

    public void start(ToneGenView aThis) {
        view = aThis;
        running = true;
        (new Thread(new SerialWriter(out, "B00000"))).start();
        (new Thread(new SerialReader(in))).start();
        System.out.println("Threads started!");

        if (output != null) {
            try {

                output.write("Y-M-D H:M:S.ms, Averaged Gyroscope, Gyro X, Gyro Y, Gyro Z, Accel X, Accel Y, Accel Z, Mag X, Mag Y, Mag Z" + System.getProperty("line.separator"));
                output.write("Trial " + String.format("%03d", ++trial) + " =================================================" + System.getProperty("line.separator"));
                output.flush();
            } catch (Exception ex1) {
                try {
                    if (!filePath.isEmpty()) {
                        output = new FileWriter(filePath, true);

                        output.write("Y-M-D H:M:S.ms, Averaged Gyroscope, Gyro X, Gyro Y, Gyro Z, Accel X, Accel Y, Accel Z, Mag X, Mag Y, Mag Z" + System.getProperty("line.separator"));
                        output.write("Trial " + String.format("%03d", ++trial) + " =================================================" + System.getProperty("line.separator"));
                        output.flush();
                    }
                } catch (Exception ex2) {
                    JOptionPane.showMessageDialog(null,
                            "Problems writing to file, did you pick the right file?",
                            "File could not be written to.",
                            JOptionPane.WARNING_MESSAGE);
                    System.out.println(ex2.toString());
                }
            }
        } else {
            System.out.println("No output! Choose a file.");
        }
    }

    public void stop() {
        try {
            if (output != null) {
                output.write("=================================================" + System.getProperty("line.separator"));
                output.flush();
                output.close();
            }
        } catch (Exception e) {
            System.out.println("forcing close");
        }

        running = false;
        (new Thread(new SerialWriter(out, "E00000"))).start();

        try {
            in.close();
        } catch (Exception ex) {
            System.out.println("forcing close...");
        }
    }

    /*
    @deprecated
     */
    public void flush() {
        (new Thread(new SerialWriter(out, "00000"))).start();
    }

    void connect(String portName) throws Exception {
        CommPortIdentifier portIdentifier = CommPortIdentifier.getPortIdentifier(portName);
        if (portIdentifier.isCurrentlyOwned()) {
            System.out.println("Error: Port is currently in use");
        } else {

            CommPort commPort = portIdentifier.open(this.getClass().getName(), 1000);

            if (commPort instanceof SerialPort) {
                SerialPort serialPort = (SerialPort) commPort;
                serialPort.setSerialPortParams(115200, SerialPort.DATABITS_8, SerialPort.STOPBITS_1, SerialPort.PARITY_NONE);
                in = serialPort.getInputStream();
                out = serialPort.getOutputStream();
            } else {
                System.out.println("Error: Only serial ports... What are you trying to do?");
            }
        }
    }

    public void save(String path) {
        try {
            if (output != null) {
                output.close();
            }
            output = new FileWriter(path, true);
            filePath = path;
        } catch (Exception ex) {
            System.out.println("Please choose a different file.\n" + ex.toString());
        }
    }

    /** */
    public static class SerialReader implements Runnable {

        InputStream in;

        public SerialReader(InputStream in) {

            this.in = in;
            sound.startTone();
        }
        //running average code
        int SIZE = 5;
        ArrayDeque<Double> xVals = new ArrayDeque<Double>();
        int xAvg = 0;

        private double runAverageX(double newX) {
            if (newX > 1000) {
                newX = 1000;
            }
            xAvg += newX / SIZE;
            //       System.out.println("calc: " + xAvg);
            xVals.push(newX);
            if (xVals.size() > SIZE) {
                xAvg -= xVals.pollLast() / SIZE;
            }
            //       System.out.println("avg: " + xAvg);
            return xAvg;
        }
        ArrayDeque<Double> yVals = new ArrayDeque<Double>();
        double yAvg = 0;

        private double runAverageY(double newY) {
            if (newY > 1000) {
                newY = 1000.0;
            }
            yAvg += newY / SIZE;
            yVals.push(newY);
            if (yVals.size() > SIZE) {
                yAvg -= yVals.pollLast() / SIZE;
            }
            return yAvg;
        }
        ArrayDeque<Double> zVals = new ArrayDeque<Double>();
        double zAvg = 0;

        private double runAverageZ(double newZ) {
            if (newZ > 1000) {
                newZ = 1000.0;
            }
            zAvg += newZ / SIZE;
            zVals.push(newZ);
            if (zVals.size() > SIZE) {
                zAvg -= zVals.pollLast() / SIZE;
            }
            return zAvg;
        }
        
        // Default values
        int xgV = -1;
        int ygV = -1;
        int zgV = -1;
        
        int xaV = -1;
        int yaV = -1;
        int zaV = -1;

        int xmV = -1;
        int ymV = -1;
        int zmV = -1;

        /* Joel's update:
         * to take the absolute value of the difference between accel(x) at two consecutive samples [e.g., accel(x) at t+1 minus accel(x) at t]                     
         * Do this for the Y and Z as well                    
         * Then sum them together (X+Y+Z) for each sample (i.e., time point)                    
         * then somehow normalize these values with those from the gyroscope, so the two have roughly the same means and standard deviations.                    
         * add the gryo and accelerometer together. 
         */
        private float runAverage(int gx, int gy, int gz, int ax, int ay, int az, int mx, int my, int mz) {
    
            // First time running average
            if (xgV == -1) {
                xgV = gx;
                ygV = gy;
                zgV = gz;
                xaV = ax;
                yaV = ay;
                zaV = az;
                xmV = mx;
                ymV = my;
                zmV = mz;
                
                return 0;
            } else {
                
                float avgV = 0;
                
                // Joel - here is where you change the formula
                // Difference between current and previous reading
                int gyroV = Math.abs(gx - xgV) + Math.abs(gy - ygV) + Math.abs(gz - zgV);
                int accelV = Math.abs(ax - xaV) + Math.abs(ay - yaV) + Math.abs(az - zaV);
                
                avgV = (gyroV + accelV) / 2.0f;
                
                // Remember old values
                xgV = gx;
                ygV = gy;
                zgV = gz;
                xaV = ax;
                yaV = ay;
                zaV = az;
                xmV = mx;
                ymV = my;
                zmV = mz;
                
                return avgV;
            }
        }
        // Reads data from device; main method for thread
        float lastVal = 0.0f;

        public void run() {

            if (view != null) {
                view.setRunning(true);
            }

            long lastTime = 0;
            while (running) {

                long timeRead = Calendar.getInstance().getTimeInMillis();
                if (Math.abs(timeRead - lastTime) > 39) {

                    lastTime = timeRead;

                    //System.out.println(Calendar.getInstance().getTimeInMillis());
                    //System.out.println(seconds + " " + Double.parseDouble(seconds));

                    int xGyro, yGyro, zGyro, xGyroRaw, yGyroRaw, zGyroRaw;
                    int xAccelRaw, yAccelRaw, zAccelRaw;
                    int xMagRaw, yMagRaw, zMagRaw;

                    // Header
                    nextByte();
                    nextByte();

                    // Actual Data
                    // Note: Abnormal data would be a -1 or a visible shift in data caused by a shift in the packet receiving

                    xGyroRaw = convN(nextByte(), nextByte());
                    yGyroRaw = convN(nextByte(), nextByte());
                    zGyroRaw = convN(nextByte(), nextByte());

                    xGyro = (int) runAverageX((double) xGyroRaw);
                    yGyro = (int) runAverageY((double) yGyroRaw); // i was just testing casting, it doesn't change anything. too lazy to change it. also too lazy to correct float-double flip flops
                    zGyro = (int) runAverageZ((double) zGyroRaw);


                    xAccelRaw = convN(nextByte(), nextByte());
                    yAccelRaw = convN(nextByte(), nextByte());
                    zAccelRaw = convN(nextByte(), nextByte());


                    xMagRaw = convN(nextByte(), nextByte());
                    yMagRaw = convN(nextByte(), nextByte());
                    zMagRaw = convN(nextByte(), nextByte());

                    // Another way to calculate average
                    // float totalGyro =  xComponent ^ 2 + yComponent ^ 2 + zComponent ^ 2;
                    // totalGyro = totalGyro / 3;
                    // totalGyro = (float) Math.sqrt(totalGyro);

                    float freq = runAverage(xGyroRaw, yGyroRaw, zGyroRaw, xAccelRaw, yAccelRaw, zAccelRaw, xMagRaw, yMagRaw, zMagRaw);
                    // Old Version:
                    // float freq = Math.abs(xGyro) + Math.abs(yGyro) + Math.abs(zGyro);

                    if (view != null) {
                        view.setProgressBar(Math.round(freq));
                        //  view.addText(fmt.format(new Date()) + ", " + freq + "\n");//xComponent + " " + yComponent + " " + zComponent + "\n");
                    }

                    if (output != null) {
                        try {
                            output.write(fmt.format(new Date()) + ", " + freq + ", ");
                            output.write(xGyroRaw + ", " + yGyroRaw + ", " + zGyroRaw + ", ");
                            output.write(xAccelRaw + ", " + yAccelRaw + ", " + zAccelRaw + ", ");
                            output.write(xMagRaw + ", " + yMagRaw + ", " + zMagRaw);
                            output.write(System.getProperty("line.separator"));
                            output.flush();
                        } catch (IOException ex) {
                            System.out.println("Problems writing to file?");
                        }
                    }

                    // Search through threshold array and apply the corresponding frequency
                    int index = -1;
                    for (Double threshold : thresholds) {
                        index++;
                        if (freq <= threshold) {
                            freq = frequencies.get(index).floatValue();
                            break;
                        }
                    }

                    /* Old frequency logic
                    if (freq <= thresholds.get(0)) {
                    freq = frequencies.get(0).floatValue();
                    } else if (freq <= thresholds.get(1)) {
                    freq = frequencies.get(1).floatValue();
                    } else if (freq <= thresholds.get(2)) {
                    freq = frequencies.get(2).floatValue();
                    } else if (freq <= thresholds.get(3)) {
                    freq = frequencies.get(3).floatValue();
                    } else if (freq <= thresholds.get(4)) {
                    freq = frequencies.get(4).floatValue();
                    }
                     */

                    try {
                        sound.changeFreq(freq);
                    } catch (InterruptedException ex) {
                        System.out.println("Problem changing frequency");
                    }
                }
            }

            if (view != null) {
                view.setRunning(false);
            }
            sound.stopTone();
        }

        // Convert 2's complement number to actual int
        int convN(int msb, int lsb) {
            String binaryString = "";

            for (int x = 0; x < 8 - Integer.toBinaryString(msb).length(); x++) {
                binaryString += "0";
            }
            binaryString += Integer.toBinaryString(msb);

            for (int x = 0; x < 8 - Integer.toBinaryString(lsb).length(); x++) {
                binaryString += "0";
            }
            binaryString += Integer.toBinaryString(lsb);

            // If it starts with a one, we have to convert it!
            if (!binaryString.startsWith("0")) {

                String temp = "";
                for (int y = 0; y < binaryString.length(); y++) {
                    if (binaryString.substring(y).startsWith("1")) {
                        temp += "0";
                    } else {
                        temp += "1";
                    }
                }

                binaryString = temp;
            }

            return Integer.parseInt(binaryString, 2);
        }

        // I created this method because errant "-1"s like to appear.
        // This means that we are reading in data too quickly when the device is starting.
        // Can we synchronize the reading with the device output?
        // How much are we trailing?
        public int nextByte() {

            int i = -1;

            try {
                i = this.in.read();
            } catch (IOException e) {
                System.out.println("Problem reading next Byte. Check device connectivity.");
            }

            if (i == -1) {
                return nextByte();
            } else {
                return i;
            }
        }
    }

    /** */
    public static class SerialWriter implements Runnable {

        OutputStream out;
        String message;

        public SerialWriter(OutputStream out, String msg) {
            this.out = out;
            message = msg;
        }

        public SerialWriter(OutputStream out) {
            this.out = out;
            message = "";
        }

        public void run() {
            if (message.length() <= 0) {
                return;
            }

            try {

                //   System.out.println("Writing: " + message + "!");
                char[] charArray = message.toCharArray();
                byte[] byteArray = CharToByteConverter.getConverter("UTF8").convertAll(charArray);

                this.out.write(byteArray);
                this.out.flush();

            } catch (Exception e) {
                // We should be properly handling device interruptions
            }
        }
    }
    // http://rgagnon.com/javadetails/java-0596.html
    static final String HEXES = "0123456789ABCDEF";

    public static String getHex(byte[] raw) {
        if (raw == null) {
            return null;
        }
        final StringBuilder hex = new StringBuilder(2 * raw.length);
        for (final byte b : raw) {
            hex.append(HEXES.charAt((b & 0xF0) >> 4)).append(HEXES.charAt((b & 0x0F)));
        }
        String s = hex.toString();

        return s;
    }
}
